///////////////////////////////////////////////////////////////////////
// title:          MilkShape 3D Model Viewer Sample
//
// copyright:      The programs and associated files contained in this
//                 distribution were developed by Mete Ciragan.  The
//                 programs are not in the public domain, but they are
//                 freely distributable without licensing fees.  These
//                 programs are provided without guarantee or warrantee
//                 expressed or implied.  Use at your own risk!
//
// email:          mciragan@gmx.net
// web:            http://www.milkshape3d.com
///////////////////////////////////////////////////////////////////////
#include "MS3DModelLoader.h"
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include "VertexType.h"
#include "Uniform.h"


void VectorIRotate(vec4 in1, mat4& in2, vec4 rez) {
    rez[0] = in1[0] * in2(0, 0) + in1[1] * in2(1, 0) + in1[2] * in2(2, 0);
    rez[1] = in1[0] * in2(0, 1) + in1[1] * in2(1, 1) + in1[2] * in2(2, 1);
    rez[2] = in1[0] * in2(0, 2) + in1[1] * in2(1, 2) + in1[2] * in2(2, 2);
}

void VectorITransform(vec4 in1, mat4& in2, vec4 rez) {
	float tmp[3] = {0,0,0}; 
    tmp[0] = in1[0] - in2(0,3);
    tmp[1] = in1[1] - in2(1,3);
    tmp[2] = in1[2] - in2(2,3);
    VectorIRotate(tmp, in2, rez);
}

void VectorRotate (const Maths::Vector3D& in1, const Maths::Matrix& in2, Maths::Vector3D& out)
{
	out.X() = in1.Dot(&in2(0,0));
	out.Y() = in1.Dot(&in2(0,1));
	out.Z() = in1.Dot(&in2(0,2));
}

void VectorTransform (const Maths::Vector3D& in1, const Maths::Matrix& in2, Maths::Vector3D& out)
{
	out.X() = in1.Dot(&in2(0,0)) +	in2(0,3);
	out.Y() = in1.Dot(&in2(0,1)) +	in2(1,3);
	out.Z() = in1.Dot(&in2(0,2)) +	in2(2,3);
}


msModel::msModel()
{
	Clear();
}

msModel::~msModel()
{
	Clear();
}

bool msModel::Load(const char *filename)
{
	std::string strFile(filename);
	int f = strFile.find_last_of('//');
	int l = strFile.find_last_of('.');
	
	m_name = strFile.substr(f+1,l-(f+1));
	FILE *fp;
	fopen_s(&fp, filename, "rb");

	if (!fp)
		return false;

	Clear();

	fseek(fp, 0, SEEK_END);
	long fileSize = ftell(fp);
	fseek(fp, 0, SEEK_SET);

	char id[10];
	fread(id, sizeof(char), 10, fp);
	if (strncmp(id, "MS3D000000", 10) != 0)
	{
		fclose(fp);
		// "This is not a valid MS3D file format!"
		return false;
	}

	int version;
	fread(&version, sizeof(int), 1, fp);
	if (version != 4)
	{
		fclose(fp);
		// "This is not a valid MS3D file version!"
		return false;
	}

	int i, j;

	// vertices
	unsigned short numVertices;
	fread(&numVertices, sizeof(unsigned short), 1, fp);
	m_vertices.resize(numVertices);
	for (i = 0; i < numVertices; i++)
	{
		fread(&m_vertices[i].flags, sizeof(unsigned char), 1, fp);
		fread(&m_vertices[i].vertex, sizeof(float), 3, fp);
		fread(&m_vertices[i].boneId, sizeof(char), 1, fp);
		fread(&m_vertices[i].referenceCount, sizeof(unsigned char), 1, fp);
	}

	// triangles
	unsigned short numTriangles;
	fread(&numTriangles, sizeof(unsigned short), 1, fp);
	m_triangles.resize(numTriangles);
	for (i = 0; i < numTriangles; i++)
	{
		fread(&m_triangles[i].flags, sizeof(unsigned short), 1, fp);
		fread(m_triangles[i].vertexIndices, sizeof(unsigned short), 3, fp);
		fread(m_triangles[i].vertexNormals, sizeof(float), 3 * 3, fp);
	    fread(m_triangles[i].s, sizeof(float), 3, fp);
	    fread(m_triangles[i].t, sizeof(float), 3, fp);
		fread(&m_triangles[i].smoothingGroup, sizeof(unsigned char), 1, fp);
		fread(&m_triangles[i].groupIndex, sizeof(unsigned char), 1, fp);

		// TODO: calculate triangle normal
	}

	// groups
	unsigned short numGroups;
	fread(&numGroups, sizeof(unsigned short), 1, fp);
	m_groups.resize(numGroups);
	for (i = 0; i < numGroups; i++)
	{
		fread(&m_groups[i].flags, sizeof(unsigned char), 1, fp);
		fread(m_groups[i].name, sizeof(char), 32, fp);

		unsigned short numGroupTriangles;
		fread(&numGroupTriangles, sizeof(unsigned short), 1, fp);
		m_groups[i].triangleIndices.resize(numGroupTriangles);
		if (numGroupTriangles > 0)
			fread(&m_groups[i].triangleIndices[0], sizeof(unsigned short), numGroupTriangles, fp);

		fread(&m_groups[i].materialIndex, sizeof(char), 1, fp);
	}

	// materials
	unsigned short numMaterials;
	fread(&numMaterials, sizeof(unsigned short), 1, fp);
	m_materials.resize(numMaterials);
	for (i = 0; i < numMaterials; i++)
	{
		fread(m_materials[i].name, sizeof(char), 32, fp);
		fread(&m_materials[i].ambient, sizeof(float), 4, fp);
		fread(&m_materials[i].diffuse, sizeof(float), 4, fp);
		fread(&m_materials[i].specular, sizeof(float), 4, fp);
		fread(&m_materials[i].emissive, sizeof(float), 4, fp);
		fread(&m_materials[i].shininess, sizeof(float), 1, fp);
        fread(&m_materials[i].transparency, sizeof(float), 1, fp);
		fread(&m_materials[i].mode, sizeof(unsigned char), 1, fp);
        fread(m_materials[i].texture, sizeof(char), MAX_TEXTURE_FILENAME_SIZE, fp);
        fread(m_materials[i].alphamap, sizeof(char), MAX_TEXTURE_FILENAME_SIZE, fp);

		// set alpha
		m_materials[i].ambient[3] = m_materials[i].transparency;
		m_materials[i].diffuse[3] = m_materials[i].transparency;
		m_materials[i].specular[3] = m_materials[i].transparency;
		m_materials[i].emissive[3] = m_materials[i].transparency;
	}

	// animation
	fread(&m_animationFps, sizeof(float), 1, fp);
	if (m_animationFps < 1.0f)
		m_animationFps = 1.0f;
	fread(&m_currentTime, sizeof(float), 1, fp);
	fread(&m_totalFrames, sizeof(int), 1, fp);

	// joints
	unsigned short numJoints;
	fread(&numJoints, sizeof(unsigned short), 1, fp);
	m_joints.resize(numJoints);
	for (i = 0; i < numJoints; i++)
	{
		fread(&m_joints[i].flags, sizeof(unsigned char), 1, fp);
		fread(m_joints[i].name, sizeof(char), 32, fp);
		fread(m_joints[i].parentName, sizeof(char), 32, fp);
        fread(m_joints[i].rot, sizeof(float), 3, fp);
        fread(m_joints[i].pos, sizeof(float), 3, fp);
    
		unsigned short numKeyFramesRot;
		fread(&numKeyFramesRot, sizeof(unsigned short), 1, fp);
		m_joints[i].rotationKeys.resize(numKeyFramesRot);

		unsigned short numKeyFramesPos;
		fread(&numKeyFramesPos, sizeof(unsigned short), 1, fp);
		m_joints[i].positionKeys.resize(numKeyFramesPos);

		// the frame time is in seconds, so multiply it by the animation fps, to get the frames
		// rotation channel
		for (j = 0; j < numKeyFramesRot; j++)
		{
			fread(&m_joints[i].rotationKeys[j].time, sizeof(float), 1, fp);
			fread(&m_joints[i].rotationKeys[j].key, sizeof(float), 3, fp);
			m_joints[i].rotationKeys[j].time *= m_animationFps;
		}

		// translation channel
		for (j = 0; j < numKeyFramesPos; j++)
		{
			fread(&m_joints[i].positionKeys[j].time, sizeof(float), 1, fp);
			fread(&m_joints[i].positionKeys[j].key, sizeof(float), 3, fp);
			m_joints[i].positionKeys[j].time *= m_animationFps;
		}
	}

	// comments
	long filePos = ftell(fp);
	if (filePos < fileSize)
	{
		int subVersion = 0;
		fread(&subVersion, sizeof(int), 1, fp);
		if (subVersion == 1)
		{
			int numComments = 0;
			size_t commentSize = 0;

			// group comments
			fread(&numComments, sizeof(int), 1, fp); 
			for (i = 0; i < numComments; i++)
			{
				int index;
				fread(&index, sizeof(int), 1, fp);
				std::vector<char> comment;
				fread(&commentSize, sizeof(size_t), 1, fp);
				comment.resize(commentSize);
				if (commentSize > 0)
					fread(&comment[0], sizeof(char), commentSize, fp);
				if (index >= 0 && index < (int) m_groups.size())
					m_groups[index].comment = comment;
			}

			// material comments
			fread(&numComments, sizeof(int), 1, fp); 
			for (i = 0; i < numComments; i++)
			{
				int index;
				fread(&index, sizeof(int), 1, fp);
				std::vector<char> comment;
				fread(&commentSize, sizeof(size_t), 1, fp);
				comment.resize(commentSize);
				if (commentSize > 0)
					fread(&comment[0], sizeof(char), commentSize, fp);
				if (index >= 0 && index < (int) m_materials.size())
					m_materials[index].comment = comment;
			}

			// joint comments
			fread(&numComments, sizeof(int), 1, fp); 
			for (i = 0; i < numComments; i++)
			{
				int index;
				fread(&index, sizeof(int), 1, fp);
				std::vector<char> comment;
				fread(&commentSize, sizeof(size_t), 1, fp);
				comment.resize(commentSize);
				if (commentSize > 0)
					fread(&comment[0], sizeof(char), commentSize, fp);
				if (index >= 0 && index < (int) m_joints.size())
					m_joints[index].comment = comment;
			}

			// model comments
			fread(&numComments, sizeof(int), 1, fp);
			if (numComments == 1)
			{
				std::vector<char> comment;
				fread(&commentSize, sizeof(size_t), 1, fp);
				comment.resize(commentSize);
				if (commentSize > 0)
					fread(&comment[0], sizeof(char), commentSize, fp);
				m_comment = comment;
			}
		}
		else
		{
			// "Unknown subversion for comments %d\n", subVersion);
		}
	}

	// vertex extra
	filePos = ftell(fp);
	if (filePos < fileSize)
	{
		int subVersion = 0;
		fread(&subVersion, sizeof(int), 1, fp);
		if (subVersion == 2)
		{
			for (int i = 0; i < numVertices; i++)
			{
				fread(&m_vertices[i].boneIds[0], sizeof(char), 3, fp);
				fread(&m_vertices[i].weights[0], sizeof(unsigned char), 3, fp);
				fread(&m_vertices[i].extra, sizeof(unsigned int), 1, fp);
			}
		}
		else if (subVersion == 1)
		{
			for (int i = 0; i < numVertices; i++)
			{
				fread(&m_vertices[i].boneIds[0], sizeof(char), 3, fp);
				fread(&m_vertices[i].weights[0], sizeof(unsigned char), 3, fp);
			}
		}
		else
		{
			// "Unknown subversion for vertex extra %d\n", subVersion);
		}
	}

	// joint extra
	filePos = ftell(fp);
	if (filePos < fileSize)
	{
		int subVersion = 0;
		fread(&subVersion, sizeof(int), 1, fp);
		if (subVersion == 1)
		{
			for (int i = 0; i < numJoints; i++)
			{
				fread(&m_joints[i].color, sizeof(float), 3, fp);
			}
		}
		else
		{
			// "Unknown subversion for joint extra %d\n", subVersion);
		}
	}

	// model extra
	filePos = ftell(fp);
	if (filePos < fileSize)
	{
		int subVersion = 0;
		fread(&subVersion, sizeof(int), 1, fp);
		if (subVersion == 1)
		{
			fread(&m_jointSize, sizeof(float), 1, fp);
			fread(&m_transparencyMode, sizeof(int), 1, fp);
			fread(&m_alphaRef, sizeof(float), 1, fp);
		}
		else
		{
			//"Unknown subversion for model extra %d\n", subVersion);
		}
	}

	fclose(fp);
	
	SetupJoints();
	SetFrame(-1.0f);

	//Maths::Vector3D tmp, vert;
	//int jointIndices[4], jointWeights[4];

	//for (int i = 0; i < m_vertices.size(); ++i)
	//{
	//	ms3d_vertex_t* vertex = &m_vertices[i];
	//	FillJointIndicesAndWeights(vertex, jointIndices, jointWeights);

	//	float weights[4] = { (float) jointWeights[0] / 100.0f, (float) jointWeights[1] / 100.0f, (float) jointWeights[2] / 100.0f, (float) jointWeights[3] / 100.0f };

	//	// add weighted vertices
	//	Maths::Vector3D newVert;
	//	for (int i = 0; i < 4; i++)
	//	{
	//		ms3d_joint_t *joint = &m_joints[jointIndices[i]];
	//		Maths::Vector3D vert;
	//	
	//		VectorITransform(vertex->vertex, joint->matGlobalSkeleton, &(vert.X()));

	//		// vert = joint->matGlobalSkeleton.getInvert() * vertex->vertex;
	//		
	//		newVert.X() += vert.X() * weights[i];
	//		newVert.Y() += vert.Y() * weights[i];
	//		newVert.Z() += vert.Z() * weights[i];
	//	}

	//	vertex->vertex[0] = newVert.X();
	//	vertex->vertex[1] = newVert.Y();
	//	vertex->vertex[2] = newVert.Z();
	//}


	return true;
}

void msModel::Clear()
{
	m_vertices.clear();
	m_triangles.clear();
	m_groups.clear();
	m_materials.clear();
	m_animationFps = 24.0f;
	m_currentTime = 1.0f;
	m_totalFrames = 30;
	m_joints.clear();
	m_comment.clear();
	m_jointSize = 1.0f;
	m_transparencyMode = TRANSPARENCY_MODE_SIMPLE;
	m_alphaRef = 0.5f;
}

std::string msModel::GetName()
{
	return m_name;
}

int msModel::GetNumGroups() const
{
	return (int) m_groups.size();
}

ms3d_group_t *msModel::GetGroup(int index)
{
	return &m_groups[index];
}

int msModel::GetNumTriangles() const
{
	return (int) m_triangles.size();
}

ms3d_triangle_t *msModel::GetTriangle(int index)
{
	return &m_triangles[index];
}

int msModel::GetNumVertices() const
{
	return (int) m_vertices.size();
}

ms3d_vertex_t *msModel::GetVertex(int index)
{
	return &m_vertices[index];
}

int msModel::GetNumMaterials() const
{
	return (int) m_materials.size();
}

ms3d_material_t *msModel::GetMaterial(int index)
{
	return &m_materials[index];
}

int msModel::GetNumJoints() const
{
	return (int) m_joints.size();
}

ms3d_joint_t *msModel::GetJoint(int index)
{
	return &m_joints[index];
}

float msModel::GetJointSize() const
{
	return m_jointSize;
}

int msModel::GetTransparencyMode() const
{
	return m_transparencyMode;
}

float msModel::GetAlphaRef() const
{
	return m_alphaRef;
}

int msModel::FindJointByName(const char *name)
{
	for (size_t i = 0; i < m_joints.size(); i++)
	{
		if (strcmp(m_joints[i].name, name) == 0)
			return i;
	}

	return -1;
}

void msModel::SetupJoints()
{
	for (size_t i = 0; i < m_joints.size(); i++)
	{
		ms3d_joint_t *joint = &m_joints[i];
		joint->parentIndex = FindJointByName(joint->parentName);
	}

	for (size_t i = 0; i < m_joints.size(); i++)
	{
		ms3d_joint_t *joint = &m_joints[i];

		Maths::Quaternion quat(Maths::Vector3D(joint->rot));
		joint->matLocalSkeleton.fromHeadPitchRoll(joint->rot[0], joint->rot[1], joint->rot[2]);
		// joint->matLocalSkeleton = quat.GetMatrix();

		// AngleMatrix(joint->rot, joint->matLocalSkeleton);
		joint->matLocalSkeleton(0, 3) = joint->pos[0];
		joint->matLocalSkeleton(1, 3) = joint->pos[1];
		joint->matLocalSkeleton(2, 3) = joint->pos[2];
		// joint->matLocalSkeleton.Translate(Maths::Vector3D(joint->pos));

		if (joint->parentIndex == -1)
		{
			joint->matGlobalSkeleton = joint->matLocalSkeleton;
			// memcpy(joint->matGlobalSkeleton, joint->matLocalSkeleton, sizeof(joint->matGlobalSkeleton));
		}
		else
		{
			ms3d_joint_t *parentJoint = &m_joints[joint->parentIndex];
			joint->matGlobalSkeleton = joint->matLocalSkeleton * parentJoint->matGlobalSkeleton;
			// R_ConcatTransforms(parentJoint->matGlobalSkeleton, joint->matLocalSkeleton, joint->matGlobalSkeleton);
		}

		SetupTangents();
	}
}

void msModel::SetupTangents()
{
	for (size_t j = 0; j < m_joints.size(); j++)
	{
		ms3d_joint_t *joint = &m_joints[j];
		int numPositionKeys = (int) joint->positionKeys.size();
		joint->tangents.resize(numPositionKeys);

		// clear all tangents (zero derivatives)
		for (int k = 0; k < numPositionKeys; k++)
		{
			joint->tangents[k].tangentIn[0] = 0.0f;
			joint->tangents[k].tangentIn[1] = 0.0f;
			joint->tangents[k].tangentIn[2] = 0.0f;
			joint->tangents[k].tangentOut[0] = 0.0f;
			joint->tangents[k].tangentOut[1] = 0.0f;
			joint->tangents[k].tangentOut[2] = 0.0f;
		}

		// if there are more than 2 keys, we can calculate tangents, otherwise we use zero derivatives
		if (numPositionKeys > 2)
		{
			for (int k = 0; k < numPositionKeys; k++)
			{
				// make the curve tangents looped
				int k0 = k - 1;
				if (k0 < 0)
					k0 = numPositionKeys - 1;
				int k1 = k;
				int k2 = k + 1;
				if (k2 >= numPositionKeys)
					k2 = 0;

				// calculate the tangent, which is the vector from key[k - 1] to key[k + 1]
				float tangent[3];
				tangent[0] = (joint->positionKeys[k2].key[0] - joint->positionKeys[k0].key[0]);
				tangent[1] = (joint->positionKeys[k2].key[1] - joint->positionKeys[k0].key[1]);
				tangent[2] = (joint->positionKeys[k2].key[2] - joint->positionKeys[k0].key[2]);

				// weight the incoming and outgoing tangent by their time to avoid changes in speed, if the keys are not within the same interval
				float dt1 = joint->positionKeys[k1].time - joint->positionKeys[k0].time;
				float dt2 = joint->positionKeys[k2].time - joint->positionKeys[k1].time;
				float dt = dt1 + dt2;
				joint->tangents[k1].tangentIn[0] = tangent[0] * dt1 / dt;
				joint->tangents[k1].tangentIn[1] = tangent[1] * dt1 / dt;
				joint->tangents[k1].tangentIn[2] = tangent[2] * dt1 / dt;

				joint->tangents[k1].tangentOut[0] = tangent[0] * dt2 / dt;
				joint->tangents[k1].tangentOut[1] = tangent[1] * dt2 / dt;
				joint->tangents[k1].tangentOut[2] = tangent[2] * dt2 / dt;
			}
		}
	}
}

void msModel::SetFrame(float frame)
{
	if (frame < 0.0f)
	{
		for (size_t i = 0; i < m_joints.size(); i++)
		{
			ms3d_joint_t *joint = &m_joints[i];
			joint->matLocal = joint->matLocalSkeleton;
			joint->matGlobal = joint->matGlobalSkeleton;
			
			// memcpy(joint->matLocal, joint->matLocalSkeleton, sizeof(joint->matLocal));
			// memcpy(joint->matGlobal, joint->matGlobalSkeleton, sizeof(joint->matGlobal));
		}
	}
	else
	{
		for (size_t i = 0; i < m_joints.size(); i++)
		{
			EvaluateJoint(i, frame);
		}
	}

	m_currentTime = frame;
}

void msModel::EvaluateJoint(int index, float frame)
{
	ms3d_joint_t *joint = &m_joints[index];

	//
	// calculate joint animation matrix, this matrix will animate matLocalSkeleton
	//
	Maths::Vector3D pos(0.0f, 0.0f, 0.0f);
	int numPositionKeys = (int) joint->positionKeys.size();
	if (numPositionKeys > 0)
	{
		int i1 = -1;
		int i2 = -1;

		// find the two keys, where "frame" is in between for the position channel
		for (int i = 0; i < (numPositionKeys - 1); i++)
		{
			if (frame >= joint->positionKeys[i].time && frame < joint->positionKeys[i + 1].time)
			{
				i1 = i;
				i2 = i + 1;
				break;
			}
		}

		// if there are no such keys
		if (i1 == -1 || i2 == -1)
		{
			// either take the first
			if (frame < joint->positionKeys[0].time)
			{
				pos = Maths::Vector3D(joint->positionKeys[0].key[0], joint->positionKeys[0].key[1], joint->positionKeys[0].key[2]);
			}
			
			// or the last key
			else if (frame >= joint->positionKeys[numPositionKeys - 1].time)
			{
				pos = Maths::Vector3D(joint->positionKeys[numPositionKeys - 1].key[0], joint->positionKeys[numPositionKeys - 1].key[1], joint->positionKeys[numPositionKeys - 1].key[2]);
			}
		}

		// there are such keys, so interpolate using hermite interpolation
		else
		{
			ms3d_keyframe_t *p0 = &joint->positionKeys[i1];
			ms3d_keyframe_t *p1 = &joint->positionKeys[i2];
			ms3d_tangent_t *m0 = &joint->tangents[i1];
			ms3d_tangent_t *m1 = &joint->tangents[i2];

			// normalize the time between the keys into [0..1]
			float t = (frame - joint->positionKeys[i1].time) / (joint->positionKeys[i2].time - joint->positionKeys[i1].time);
			float t2 = t * t;
			float t3 = t2 * t;

			// calculate hermite basis
			float h1 =  2.0f * t3 - 3.0f * t2 + 1.0f;
			float h2 = -2.0f * t3 + 3.0f * t2;
			float h3 =         t3 - 2.0f * t2 + t;
			float h4 =         t3 -        t2;

			// do hermite interpolation
			pos = Maths::Vector3D( h1 * p0->key[0] + h3 * m0->tangentOut[0] + h2 * p1->key[0] + h4 * m1->tangentIn[0], h1 * p0->key[1] + h3 * m0->tangentOut[1] + h2 * p1->key[1] + h4 * m1->tangentIn[1], h1 * p0->key[2] + h3 * m0->tangentOut[2] + h2 * p1->key[2] + h4 * m1->tangentIn[2]);
		}
	}

	Maths::Quaternion quat(0.0f, 0.0f, 0.0f, 1.0f);
	int numRotationKeys = (int) joint->rotationKeys.size();
	if (numRotationKeys > 0)
	{
		int i1 = -1;
		int i2 = -1;

		// find the two keys, where "frame" is in between for the rotation channel
		for (int i = 0; i < (numRotationKeys - 1); i++)
		{
			if (frame >= joint->rotationKeys[i].time && frame < joint->rotationKeys[i + 1].time)
			{
				i1 = i;
				i2 = i + 1;
				break;
			}
		}

		// if there are no such keys
		if (i1 == -1 || i2 == -1)
		{
			// either take the first key
			if (frame < joint->rotationKeys[0].time)
			{
				// AngleQuaternion(joint->rotationKeys[0].key, quat);
				quat.Set(joint->rotationKeys[0].key[0], joint->rotationKeys[0].key[1], joint->rotationKeys[0].key[2]);
			}

			// or the last key
			else if (frame >= joint->rotationKeys[numRotationKeys - 1].time)
			{
				// AngleQuaternion(joint->rotationKeys[numRotationKeys - 1].key, quat);
				quat.Set(joint->rotationKeys[numRotationKeys - 1].key[0], joint->rotationKeys[numRotationKeys - 1].key[1], joint->rotationKeys[numRotationKeys - 1].key[2]);
			}
		}

		// there are such keys, so do the quaternion slerp interpolation
		else
		{
			float t = (frame - joint->rotationKeys[i1].time) / (joint->rotationKeys[i2].time - joint->rotationKeys[i1].time);
			Maths::Quaternion q1;
			q1.Set(joint->rotationKeys[i1].key[0], joint->rotationKeys[i1].key[1], joint->rotationKeys[i1].key[2]);
			// AngleQuaternion(joint->rotationKeys[i1].key, q1);
			Maths::Quaternion q2;
			// AngleQuaternion(joint->rotationKeys[i2].key, q2);
			q1.Set(joint->rotationKeys[i2].key[0], joint->rotationKeys[i2].key[1], joint->rotationKeys[i2].key[2]);
			// QuaternionSlerp(q1, q2, t, quat);
			quat.Slerp(q1, q2, t);
		}
	}

	// make a matrix from pos/quat
	Maths::Matrix matAnimate;
	matAnimate = quat.GetMatrix();
	// QuaternionMatrix(quat, matAnimate);
	matAnimate.Translate(pos);
	//matAnimate[0][3] = pos[0];
	//matAnimate[1][3] = pos[1];
	//matAnimate[2][3] = pos[2];

	// animate the local joint matrix using: matLocal = matLocalSkeleton * matAnimate
	joint->matLocal = matAnimate * joint->matLocalSkeleton;
	// R_ConcatTransforms(joint->matLocalSkeleton, matAnimate, joint->matLocal);

	// build up the hierarchy if joints
	// matGlobal = matGlobal(parent) * matLocal
	if (joint->parentIndex == -1)
	{
		joint->matGlobal = joint->matLocal;
		// memcpy(joint->matGlobal, joint->matLocal, sizeof(joint->matGlobal));
	}
	else
	{
		ms3d_joint_t *parentJoint = &m_joints[joint->parentIndex];
		joint->matGlobal = parentJoint->matGlobal * joint->matLocal;
		// R_ConcatTransforms(parentJoint->matGlobal, joint->matLocal, joint->matGlobal);
	}
}

float msModel::GetAnimationFps() const
{
	return m_animationFps;
}

float msModel::GetCurrentFrame() const
{
	return m_currentTime;
}

int msModel::GetTotalFrames() const
{
	return m_totalFrames;
}

void msModel::TransformVertex(const ms3d_vertex_t *vertex, Maths::Vector3D& out) const
{
	int jointIndices[4], jointWeights[4];
	FillJointIndicesAndWeights(vertex, jointIndices, jointWeights);

	if (jointIndices[0] < 0 || jointIndices[0] >= (int) m_joints.size() || m_currentTime < 0.0f)
	{
		out = vertex->vertex;
		//out[0] = vertex->vertex[0];
		//out[1] = vertex->vertex[1];
		//out[2] = vertex->vertex[2];
	}
	else
	{
		// count valid weights
		int numWeights = 0;
		for (int i = 0; i < 4; i++)
		{
			if (jointWeights[i] > 0 && jointIndices[i] >= 0 && jointIndices[i] < (int) m_joints.size())
				++numWeights;
			else
				break;
		}

		// init
		// out[0] = 0.0f;
		// out[1] = 0.0f;
		// out[2] = 0.0f;

		float weights[4] = { (float) jointWeights[0] / 100.0f, (float) jointWeights[1] / 100.0f, (float) jointWeights[2] / 100.0f, (float) jointWeights[3] / 100.0f };
		if (numWeights == 0)
		{
			numWeights = 1;
			weights[0] = 1.0f;
		}
		// add weighted vertices
		for (int i = 0; i < numWeights; i++)
		{
			const ms3d_joint_t *joint = &m_joints[jointIndices[i]];
			Maths::Vector3D tmp, vert;

			vert = joint->matGlobalSkeleton.getInvert() * vertex->vertex;
			// VectorITransform(vertex->vertex, joint->matGlobalSkeleton, tmp);
			// VectorTransform(tmp, joint->matGlobal, vert);
			
			out.X() += vert.X() * weights[i];
			out.Y() += vert.Y() * weights[i];
			out.Z() += vert.Z() * weights[i];

			//out[0] += vert[0] * weights[i];
			//out[1] += vert[1] * weights[i];
			//out[2] += vert[2] * weights[i];
		}
	}
}

void msModel::TransformNormal(const ms3d_vertex_t *vertex, const Maths::Vector3D& normal, Maths::Vector3D& out) const
{
	int jointIndices[4], jointWeights[4];
	FillJointIndicesAndWeights(vertex, jointIndices, jointWeights);

	if (jointIndices[0] < 0 || jointIndices[0] >= (int) m_joints.size() || m_currentTime < 0.0f)
	{
		// out[0] = normal[0];
		// out[1] = normal[1];
		// out[2] = normal[2];
	}
	else
	{
		// count valid weights
		int numWeights = 0;
		for (int i = 0; i < 4; i++)
		{
			if (jointWeights[i] > 0 && jointIndices[i] >= 0 && jointIndices[i] < (int) m_joints.size())
				++numWeights;
			else
				break;
		}

		// init
		// out[0] = 0.0f;
		// out[1] = 0.0f;
		// out[2] = 0.0f;

		float weights[4] = { (float) jointWeights[0] / 100.0f, (float) jointWeights[1] / 100.0f, (float) jointWeights[2] / 100.0f, (float) jointWeights[3] / 100.0f };
		if (numWeights == 0)
		{
			numWeights = 1;
			weights[0] = 1.0f;
		}
		// add weighted vertices
		for (int i = 0; i < numWeights; i++)
		{
			const ms3d_joint_t *joint = &m_joints[jointIndices[i]];
			Maths::Vector3D tmp, norm;
			tmp = joint->matGlobalSkeleton.getInvert() * normal;
			// VectorIRotate(normal, joint->matGlobalSkeleton, tmp);
			// VectorRotate(tmp, joint->matGlobal, norm);

			out.X() += norm.X() * weights[i];
			out.Y() += norm.Y() * weights[i];
			out.Z() += norm.Z() * weights[i];
			//out[0] += norm[0] * weights[i];
			//out[1] += norm[1] * weights[i];
			//out[2] += norm[2] * weights[i];
		}
	}
}

void msModel::FillJointIndicesAndWeights(const ms3d_vertex_t *vertex, int jointIndices[4], int jointWeights[4]) const
{
	jointIndices[0] = vertex->boneId;
	jointIndices[1] = vertex->boneIds[0];
	jointIndices[2] = vertex->boneIds[1];
	jointIndices[3] = vertex->boneIds[2];
	jointWeights[0] = 100;
	jointWeights[1] = 0;
	jointWeights[2] = 0;
	jointWeights[3] = 0;
	if (vertex->weights[0] != 0 || vertex->weights[1] != 0 || vertex->weights[2] != 0)
	{
		jointWeights[0] = vertex->weights[0];
		jointWeights[1] = vertex->weights[1];
		jointWeights[2] = vertex->weights[2];
		jointWeights[3] = 100 - (vertex->weights[0] + vertex->weights[1] + vertex->weights[2]);
	}
}



void ConvertToMdlFile( msModel& model, MdlFile& mdlFile )
{
	int groupNum = model.GetNumGroups();
	mdlFile.meshes.resize(groupNum);
	mdlFile.modelName = model.GetName();
	mdlFile.skeletonName = model.GetName();

	for (int groupIndex = 0; groupIndex < groupNum; ++groupIndex)
	{
		ms3d_group_t* group = model.GetGroup(groupIndex);

		MdlFileMesh& mdlMesh = mdlFile.meshes[groupIndex];

		mdlMesh.parameters.push_back(Type(0, 3, enumPosition));
		mdlMesh.parameters.push_back(Type(12, 3, enumNormal));
		mdlMesh.parameters.push_back(Type(24, 2, enumTexCoord0));
		mdlMesh.parameters.push_back(Type(32, 4, enumAttrib0));
		mdlMesh.parameters.push_back(Type(48, 4, enumAttrib1));

		mdlMesh.vertexCount = group->triangleIndices.size() * 3;
		mdlMesh.vertexData.resize(mdlMesh.vertexCount * 64);

		unsigned char* vertPtr = &mdlMesh.vertexData[0];
		for (std::vector<unsigned short>::iterator triangleIndex = group->triangleIndices.begin(); triangleIndex != group->triangleIndices.end(); ++triangleIndex)
		{
			ms3d_triangle_t* triangle = model.GetTriangle(*triangleIndex);
			
			for (int vertexIndex = 0; vertexIndex < 3; ++vertexIndex)
			{
				ms3d_vertex_t* vertex = model.GetVertex(triangle->vertexIndices[vertexIndex]);
				int jointIndices[4], jointWeights[4];
				model.FillJointIndicesAndWeights(vertex, jointIndices, jointWeights);
			
				memcpy(vertPtr, vertex->vertex, 3 * sizeof(float));
				vertPtr += 12;
				memcpy(vertPtr, triangle->vertexNormals[vertexIndex], 3 * sizeof(float));
				vertPtr += 12;
				memcpy(vertPtr, &(triangle->s[vertexIndex]), 1 * sizeof(float));
				vertPtr += 4;
				memcpy(vertPtr, &(triangle->t[vertexIndex]), 1 * sizeof(float));
				vertPtr += 4;
				for (int i = 0; i < 4; ++i)
					((float*)vertPtr)[i] = (float)(jointIndices[i]);
				vertPtr += 16;
				for (int i = 0; i < 4; ++i)
				{
					float weight = (float)(jointWeights[i])/100.0f;
					((float*)vertPtr)[i] = weight;
				}
				vertPtr += 16;
			}
		}
		
		MdlFileMaterial& mdlMat = mdlMesh.material;
		ms3d_material_t* msMat = model.GetMaterial(group->materialIndex);

		memcpy(msMat->ambient, mdlMat.ambient, 4 * sizeof(float));
		memcpy(msMat->diffuse, mdlMat.diffuse, 4 * sizeof(float));
		memcpy(msMat->emissive, mdlMat.emmisive, 4 * sizeof(float));
		memcpy(msMat->specular, mdlMat.specular, 4 * sizeof(float));
		memcpy(&msMat->shininess, &mdlMat.shininess, sizeof(float));
		mdlMat.textureNames.push_back(msMat->texture);
	}
}
